#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_task.h"
#include "driver/i2c.h"
#include "driver/gpio.h"

#include "hh_util.h"
#include "sensor_tf_luna_res.h"
#include "sensor_tf_luna.h"

#define DBG_FMT "LUNA"

#define TF_LUNA_TSK_STACK_SIZE      ESP_TASK_MAIN_STACK
#define TF_LUNA_TSK_PRIORITY        1
#define TF_LUNA_TSK_PERIOD_MS       200

#define I2C_MASTER_SCL_IO           22                          /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO           21                          /*!< GPIO number used for I2C master data */
#define I2C_MASTER_NUM              0                           /*!< I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip */
#define I2C_MASTER_FREQ_HZ          TF_LUNA_I2C_MAX_FREQ        /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE   0                           /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0                           /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS       1000

typedef struct _tf_luna_ctx {
    TaskHandle_t    task_handle;
    bool            exit_task;
    bool            exited;
    tf_luna_info_t  info;
} tf_luna_ctx_t;

/*============================================================================*/
/*                          helper functions define                           */
/*============================================================================*/

/**
 * @brief Read a sequence of bytes from a TF_LUNA sensor registers
 */
static esp_err_t tf_luna_register_read(uint8_t reg_addr, uint8_t *data, size_t len)
{
    return i2c_master_write_read_device(I2C_MASTER_NUM, TF_LUNA_SENSOR_ADDR, &reg_addr, 1, data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
}

/**
 * @brief Write a byte to a TF_LUNA sensor register
 */
esp_err_t tf_luna_register_write_byte(uint8_t reg_addr, uint8_t data)
{
    int ret;
    uint8_t write_buf[2] = {reg_addr, data};

    ret = i2c_master_write_to_device(I2C_MASTER_NUM, TF_LUNA_SENSOR_ADDR, write_buf, sizeof(write_buf), I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);

    return ret;
}

/**
 * @brief i2c master initialization
 */
static esp_err_t i2c_master_init(void)
{
    int i2c_master_port = I2C_MASTER_NUM;

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };

    i2c_param_config(i2c_master_port, &conf);

    return i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}


void _tf_luna_sensor_task_entry(void *priv)
{
    if (!priv) {
        ESP_LOGE_FUNC(DBG_FMT, "priv is NULL, task exit!");
        return;
    }
    tf_luna_ctx_t *p_ctx = (tf_luna_ctx_t *)priv;

    uint8_t  dist_reg[TF_LUNA_DIST_REG_LEN] = {0};
    uint16_t dist_val = 0;
    uint8_t  amp_reg[TF_LUNA_AMP_REG_LEN] = {0};
    uint16_t amp_val = 0;
    uint8_t  temp_reg[TF_LUNA_TEMP_REG_LEN] = {0};
    uint16_t temp_val = 0;
    uint8_t  module_sn[TF_LUNA_MODULE_SN_REG_LEN + 1] = {0};

    ESP_ERROR_CHECK(i2c_master_init());
    ESP_LOGI_FUNC(DBG_FMT, "I2C initialized successfully");

    while (!p_ctx->exit_task) {
        vTaskDelay(TF_LUNA_TSK_PERIOD_MS / portTICK_PERIOD_MS);

        ESP_ERROR_CHECK(tf_luna_register_read(TF_LUNA_DIST_REG_ADDR, dist_reg, TF_LUNA_DIST_REG_LEN));
        dist_val = (uint16_t)dist_reg[1] << 8 | dist_reg[0];
        ESP_LOGV_FUNC(DBG_FMT, "TF-Luna dist value is %d cm", dist_val);

        ESP_ERROR_CHECK(tf_luna_register_read(TF_LUNA_AMP_REG_ADDR, amp_reg, TF_LUNA_AMP_REG_LEN));
        amp_val = (uint16_t)amp_reg[1] << 8 | amp_reg[0];
        ESP_LOGV_FUNC(DBG_FMT, "TF-Luna amp value is %d", amp_val);

        ESP_ERROR_CHECK(tf_luna_register_read(TF_LUNA_TEMP_REG_ADDR, temp_reg, TF_LUNA_TEMP_REG_LEN));
        temp_val = (uint16_t)temp_reg[1] << 8 | temp_reg[0];
        ESP_LOGV_FUNC(DBG_FMT, "TF-Luna temp value is %d x 0.01", temp_val);

        ESP_ERROR_CHECK(tf_luna_register_read(TF_LUNA_MODULE_SN_REG_ADDR, module_sn, TF_LUNA_MODULE_SN_REG_LEN));
        module_sn[TF_LUNA_MODULE_SN_REG_LEN] = 0;
        ESP_LOGV_FUNC(DBG_FMT, "TF-Luna module sn: %s", module_sn);

        p_ctx->info.dist_cm = dist_val;
        p_ctx->info.amp = amp_val;
        p_ctx->info.temperature = temp_val;
    }

    ESP_ERROR_CHECK(i2c_driver_delete(I2C_MASTER_NUM));
    ESP_LOGI_FUNC(DBG_FMT, "I2C de-initialized successfully");

    ESP_LOGI_FUNC(DBG_FMT, "tf_luna sensor task[%p] exit!", p_ctx->task_handle);
    p_ctx->exited = true;
}

esp_err_t _init_task(tf_luna_ctx_t *p_ctx)
{
    esp_err_t rlt = ESP_OK;

    if (p_ctx->task_handle) {
        ESP_LOGE_FUNC(DBG_FMT, "already create task!");
        return ESP_FAIL;
    }

    ESP_LOGI_FUNC(DBG_FMT, "start to create tf_luna sensor task");
    xTaskCreate(_tf_luna_sensor_task_entry,
                "tf_luna_tsk",
                TF_LUNA_TSK_STACK_SIZE,
                (void *)p_ctx,
                TF_LUNA_TSK_PRIORITY,
                &p_ctx->task_handle);
    if (p_ctx->task_handle) {
        ESP_LOGI_FUNC(DBG_FMT, "tf_luna sensor task create success, handle:%p", p_ctx->task_handle);
    } else {
        ESP_LOGE_FUNC(DBG_FMT, "tf_luna sensor task create failed");
        rlt = ESP_FAIL;
    }

    return rlt;
}

/*============================================================================*/
/*                             public APIs define                             */
/*============================================================================*/
esp_err_t tf_luna_init(tf_luna_ctx_t **pp_ctx)
{
    esp_err_t rlt = ESP_OK;

    tf_luna_ctx_t *p_ctx = calloc(1, sizeof(tf_luna_ctx_t));
    if (!p_ctx) {
        ESP_LOGE_FUNC(DBG_FMT, "no memory!");
        return ESP_ERR_NO_MEM;
    }

    rlt = _init_task(p_ctx);
    if (ESP_OK != rlt) {
        ESP_LOGE_FUNC(DBG_FMT, "init task failed, %d", rlt);
        goto fail;
    }

    *pp_ctx = p_ctx;
    return ESP_OK;

fail:
    free(p_ctx);
    *pp_ctx = NULL;
    return rlt;
}

void tf_luna_deinit(tf_luna_ctx_t *p_ctx)
{
    if (p_ctx) {
        p_ctx->exit_task = true;
        do {
            vTaskDelay(5 / portTICK_PERIOD_MS); // delay 5 ms
        } while (!p_ctx->exited);
        free(p_ctx);
    }
}

esp_err_t tf_luna_get_info(tf_luna_ctx_t *p_ctx, tf_luna_info_t *p_info)
{
    if (!p_ctx) {
        ESP_LOGE_FUNC(DBG_FMT, "p_ctx is NULL!");
        return ESP_ERR_INVALID_ARG;
    }

    if (!p_info) {
        ESP_LOGE_FUNC(DBG_FMT, "p_info is NULL!");
        return ESP_ERR_INVALID_ARG;
    }

    memcpy(p_info, &p_ctx->info, sizeof(tf_luna_info_t));
    return ESP_OK;
}
